#ifndef  BASICPCL_H__
#define  BASICPCL_H__
#include "Basicpcl.h"

//帮助函数
void ShowHelp(const char * program_name)
{
//   std::cout << std::endl;
//   std::cout << "Usage: " << program_name << " cloud_in.pcd" << std::endl;
//   //std::cout << "Usage: " << program_name << " cloud_out.pcd" << std::endl;
    printf("Usage: %s cloud_in.pcd ", program_name);
    printf("-h:  Show this help." );
 
}
 //预处理
int Pretreatment(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_o, const float &leaf_size, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
    // //去除NAN点
    std::vector<int>indices_;//保存去除的点的索引
    pcl::removeNaNFromPointCloud(*cloud_o,*cloud_o,indices_);
    // 下采样滤波
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(cloud_o);
    voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);
    voxel_grid.filter(*cloud_);
    return 0 ;
}
//直通滤波
int Passfilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string a, const float min, const float max, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
    // pcl::PointXYZ minPt, maxPt;
    // pcl::getMinMax3D(*cloud, minPt, maxPt);
    pcl::PassThrough<pcl::PointXYZ> passthrough;
    passthrough.setInputCloud(cloud);
    passthrough.setFilterFieldName(a);//对z轴进行操作
    passthrough.setFilterLimits(min,max);//设置直通滤波器操作范围
    //passthrough.setFilterLimitsNegative(true); //true表示保留范围内，false表示保留范围外
    passthrough.filter(*cloud_);
    return 0;
}
//半径滤波
int RadiusFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double R, int n, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
{
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    outrem.setInputCloud(cloud_in);
    outrem.setRadiusSearch(R);//设置在0.02半径的范围内找邻近点
    outrem.setMinNeighborsInRadius(n);//设置查询点的邻近点集数小于5的删除
    outrem.filter(*cloud_out);

    return 0;
}
// 统计滤波
// 创建滤波器，对每个点分析的临近点的个数设置为50 ，并将标准差的倍数设置为1  这意味着如果一
// 个点的距离超出了平均距离一个标准差以上，则该点被标记为离群点，并将它移除
int  Statsticalfilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;   	// 创建滤波器对象
    sor.setInputCloud(cloud);                   	// 设置待滤波的点云
    sor.setMeanK(200);                           	// 设置在进行统计时考虑查询点临近点数
    sor.setStddevMulThresh(1);                      // 设置判断是否为离群点的阀值
    sor.filter(*cloud_);                            	// 过滤
    return 0;
}
//欧式距离提取
int EuclideCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,
 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Eucluextra)
{
    std::cout<<"EuclideCloud"<<std::endl;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    tree->setInputCloud(cloud_in);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02);//设置近邻搜索的搜索半径
    ec.setMinClusterSize(1); //设置最小聚类尺寸
    ec.setMaxClusterSize(200);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_in);
    ec.extract(cluster_indices);

    //std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Eucluextra;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();it != cluster_indices.end();++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for(std::vector<int>::const_iterator pit = it->indices.begin();pit != it->indices.end();pit++)
        {
            cloud_cluster->points.push_back(cloud_in->points[*pit]);
        }
        cloud_cluster->width = cloud_cluster->points.size(); 
        cloud_cluster->height =1;
        cloud_cluster->is_dense = true;
            std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
        //Eucluextra.push_back(cloud_cluster);
    }

    return 0;
}
//法向量
int NormalMath( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr normals)
{
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne;//法线估计对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setSearchMethod(tree);
    ne.setInputCloud(cloud);
    ne.setRadiusSearch(50);
    ne.compute(*normals);
    return 0;
}


//旋转
int Transformx(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,float theta)
{
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    // Define a translation of 2.5 meters on the x axis.
    transform_2.translation() << 0.0, 0.0, 0.0;
    // The same rotation matrix as before; theta radians arround Z axis
    transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
    // Print the transformation
    printf ("\nMethod #2: using an Affine3f\n");
    std::cout << transform_2.matrix() << std::endl;
    pcl::transformPointCloud (*cloud_in, *cloud_out, transform_2);

    return 0;
}

int Transformy(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,float theta)
{
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    // Define a translation of 2.5 meters on the x axis.
    transform_2.translation() << 0.0, 0.0, 0.0;
    // The same rotation matrix as before; theta radians arround Z axis
    transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitY()));
    // Print the transformation
    printf ("\nMethod #2: using an Affine3f\n");
    std::cout << transform_2.matrix() << std::endl;
    pcl::transformPointCloud (*cloud_in, *cloud_out, transform_2);

    return 0;
}
int Segmentation( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr normal,const int &model, 
pcl::PointIndices::Ptr inliers_plane,pcl::ModelCoefficients::Ptr  coefficients_,  const int &Iterations ,  const double &distance)
{
    pcl::SACSegmentationFromNormals<pcl::PointXYZ,pcl::Normal> seg;//分割对象
    seg.setInputCloud(cloud);
    seg.setOptimizeCoefficients(true);
    seg.setModelType(model); 
    seg.setNormalDistanceWeight(0.1);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(Iterations);
    seg.setDistanceThreshold(distance);
    seg.setInputNormals(normal);  
    if(model == pcl::SACMODEL_CYLINDER )
    {
        seg.setRadiusLimits(0,0.1);
    }
    seg.segment(*inliers_plane,*coefficients_);
    return 0;
}
//提取分割点集

int Extractcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointIndices::Ptr inliers_, bool Negative_Extra, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliers)
{
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
    extract.setNegative(Negative_Extra);
    extract.setIndices(inliers_);
    extract.filter(*cloud_inliers);
    return 0;
}

//多平面分割提取
int SegmentModel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, 
std::vector<pcl::ModelCoefficients::Ptr>& seg_coefficients, int faces , int model, float R ,bool last)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud_in, *cloud); //cloud_in  替代 cloud
    //提前表面
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    //参数
    seg.setOptimizeCoefficients(true);
    seg.setModelType(model); // 提取模式
    seg.setMethodType(pcl::SAC_RANSAC);//采样方法
    seg.setDistanceThreshold(R);//采样半径
    seg.setMaxIterations(50);//迭代次数

    //提取索引
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    seg_coefficients.clear();//  清空  Vector
    seg_clouds.clear();

    for(int i = 0; i < faces; ++i)
    {
        printf("正在提取第%d 个\n", i+1);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        seg.setInputCloud(cloud);
        seg.segment(*inliers, *coefficients);

        extract.setInputCloud(cloud);
        extract.setIndices(inliers);// 设置分割后的内点为需要提取的点集
        extract.setNegative(false);//提取内点
        extract.filter(*cloud_filtered);
        extract.setNegative(true);//提取 外点 
        extract.filter(*cloud);

        seg_clouds.push_back(cloud_filtered); // 把分割面  存储到 seg_clouds
        seg_coefficients.push_back(coefficients);
    }
    //判断剩余  存入 
    if(last){
        seg_clouds.push_back(cloud);
    }
    
    printf("表面提取完成\n");
    return 0;
}
// 线距离 
int Distance_line(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds_line,  std::vector<float> distance)
{
    std::vector<pcl::PointXYZ> min;
    std::vector<pcl::PointXYZ> max;
    // 计算   线长 
    for(int i=0; i<seg_clouds_line.size(); i++)
    {   
        int k = 0;
        int n = 0 ;
        float pointmin =abs(seg_clouds_line[i]->points[0].x) ;
        float pointmax = abs(seg_clouds_line[i]->points[0].x);
        for(int j =0; j<seg_clouds_line[i]->size(); j++)
        {
            float pointx = seg_clouds_line[i]->points[j].x;
            if(abs(pointx)>pointmax){
                 pointmax = abs(pointx);
                 k = j;
            }
               
            if(abs(pointx)<pointmin){
                pointmin = abs(pointx);
                 n = j;
            }
                
             
        }
        min.push_back(seg_clouds_line[i]->points[n]);
        max.push_back(seg_clouds_line[i]->points[k]);

    }
    for (int m = 0; m< min.size(); m++)
    {
        float dis =  sqrt(pow(min[m].x-max[m].x, 2)+pow(min[m].y-max[m].y, 2)+pow(min[m].z-max[m].z, 2));
        distance.push_back(dis);
        printf("第%d 条线距离%f\n", m+1, dis);          
    }
    return 0;
}


// 拟合 3D 圆
int ConsensusCircle3D(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<int> &inliersCircle3D)
{
    pcl::SampleConsensusModelCircle3D<pcl::PointXYZ>::Ptr socCircle3D ( new pcl::SampleConsensusModelCircle3D<pcl::PointXYZ> (cloud));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (socCircle3D) ;
    ransac.setDistanceThreshold(1);
    ransac.computeModel();
    ransac.getInliers(inliersCircle3D);
    Eigen::VectorXf modelParas;
    ransac.getModelCoefficients(modelParas);
    //printf(modelParas);
    std::cout<<modelParas<<std::endl;
    printf("circle_coeff 3D rows = %d \t  cols = %d \n", modelParas.rows(),modelParas.cols());
    return 0;
}
//拟合 2D 圆
int ConsensusCircle2D(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<int> &inliersCircle2D)
{
    pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr model_circle2D(new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>(cloud));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_circle2D);
    ransac.setDistanceThreshold(1);
    ransac.computeModel();
    ransac.getInliers(inliersCircle2D);

    Eigen::VectorXf modelParas2D;
    ransac.getModelCoefficients(modelParas2D);
    std::cout<<"circle_coeff 2D\n"<<modelParas2D<<std::endl;

    return 0;
}

//Kdtree
int Kdtreecloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const int min, const int max, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
  //  pcl::search::
  return 0;
}

//可视化
boost::shared_ptr<pcl::visualization::PCLVisualizer>
SimpleVIS( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer>  viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));
    int v1(0);
    int v2(0);

    viewer->createViewPort(0, 0, 0.5, 1, v1);
    viewer->createViewPort(0.5, 0, 1, 1, v2);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->setBackgroundColor(0, 0, 4, v2);
   viewer->addPointCloud<pcl::PointXYZ>(cloud2, "sample cloud2", v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud1, "sample cloud1", v1);
    
    viewer->addPointCloud<pcl::PointXYZ>(cloud_in, "cloud_in", v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud1", v1);
     viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud1", v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "cloud_in", v2);

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_in", v2);//原始颜色为红色
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sample cloud1", v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud1, "1", v2);
    viewer->addPointCloud<pcl::PointXYZ>(cloud2, "2", v2);
    viewer->addCoordinateSystem(50.0);//坐标系
    viewer->initCameraParameters();

    viewer->resetCamera();
    viewer->addCoordinateSystem();

    
    return(viewer);
}

//可视化点云集 
void Visual_clouds(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds)
{
    printf("可视化\n");
    pcl::visualization::PCLVisualizer viewer ("clouds");
    for(int i = 0; i < clouds.size(); ++i )
    {
        int *rgb = new int [3];
        rgb[0] = rand() % 255;
        rgb[1] = rand() % 255;
        rgb[2] = rand() % 255;
        if(i == clouds.size()-1)
        {
            rgb[0] = 255;
            rgb[1] = 255;
            rgb[2] = 255;
        }
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_cloud(clouds[i], rgb[0], rgb[1], rgb[2]);
        viewer.addPointCloud(clouds[i], color_cloud, "clouds" + std::to_string(i));
        viewer.addCoordinateSystem();
        viewer.resetCamera();
    }
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
}
//单个显示
void Visual_cloudone(pcl::PointCloud<pcl::PointXYZ>::Ptr clouds)
{
    
    pcl::visualization::PCLVisualizer viewer ("clouds");
   
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_cloud(clouds, 0, 255, 255);
    viewer.addPointCloud(clouds, color_cloud, "clouds" );
    viewer.addCoordinateSystem(50);
    viewer.resetCamera();

    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
}


//保存点云
int Save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>clouds)
{
    printf("存储点云\n");
    for(int i = 0; i < clouds.size(); i++)
    {
        pcl::io::savePCDFileBinary( std::to_string(i+1)+".pcd", *clouds[i]);//std::to_string(i) + ".pcd"
    }

    return 0;
}


//提取边界
int Boundary_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
{
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Boundary> boundarys;
    pcl::BoundaryEstimation<pcl::PointXYZ,pcl::Normal,pcl::Boundary> est;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normEst;
    normEst.setInputCloud(cloud_in);
    normEst.setSearchMethod(tree);
    normEst.setKSearch(200); //法向估计的点数
    normEst.compute(*normals);

    est.setInputCloud(cloud_in);
    est.setInputNormals(normals);
    est.setSearchMethod(tree);
    //est.setAngleThreshold(M_PI / 4);
    //est.setRadiusSearch(50);
    est.setKSearch(40);
    est.compute(boundarys);

    pcl::PointCloud<pcl::PointXYZ>::Ptr boundPoints(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> noBoundPoints;
    int countBoundaries = 0;
    for (int i = 0; i <cloud_in->size(); ++i)
    {
        uint8_t  x = (boundarys.points[i].boundary_point);
   
        int a = static_cast<int>(x);//该函数的功能是强制类型转换
        if(a==1)
        {
            (*boundPoints).push_back(cloud_in->points[i]);
            countBoundaries++;
        }
        else{
            noBoundPoints.push_back(cloud_in->points[i]);
        }

    }
    pcl::copyPointCloud(*boundPoints,*cloud_out);
    return 0;

}



//平面 组
std::vector<std::pair<int, int >>  Group_plane(std::vector<pcl::ModelCoefficients::Ptr>& seg_coefficients)
{
    int size = seg_coefficients.size();
    std::vector<std::pair<int, int >>  groups;
    float angle = 0;
    
    for(int i = 0; i < size; i++)
    {   
        for(int j = i+1; j < size; j++ )
        {
            angle = Angle_plane(seg_coefficients[i] ,seg_coefficients[j]) ;
            if (angle < 20)
            {
                //cout << i <<"," << j <<endl;
                groups.emplace_back(i, j); // 平行2个面 i j  

            }
        }
        
    }  
    // for(int s = 0; s<groups.size() ; s++)
    // {
    //     cout << "groups....." << s <<"  "<< groups[s].first<< "   "<< groups[s].second <<endl;
    // }
    return groups;
}

//计算 两个平面夹角
float Angle_plane(pcl::ModelCoefficients::Ptr a, pcl::ModelCoefficients::Ptr b)
{
    float s2a = sqrt(pow(a->values[3],2)+ pow(a->values[4], 2) + pow(a->values[5], 2));
    float s2b = sqrt(pow(b->values[3],2)+ pow(b->values[4], 2) + pow(b->values[5], 2) );
    float c = (a->values[3]*b->values[3]+a->values[4]*b->values[4]+a->values[5]*b->values[5])/ s2a/s2b;
    float angle = acos(c)*180/M_PI;
    //cout<< angle<<endl;
    return  angle;
}

//计算面 距离
std::vector<float> Distance_plane(Eigen::Vector4f &centroid, std::vector<pcl::ModelCoefficients::Ptr> &seg_coefficients, std::vector<std::pair<int, int >> groups)
{
    printf("开始计算面距离\n");
    std::vector<float> distance;
    for(int i = 0; i < groups.size(); i++)
    {
        float ds = abs(Dis_dp(centroid, seg_coefficients[groups[i].first])-Dis_dp(centroid, seg_coefficients[groups[i].second]));
        distance.push_back(ds);
        printf("第%d 面距离%f\n", i+1 , ds);
    }
    return distance;
}
//点到平面距离
float Dis_dp(Eigen::Vector4f &dot, pcl::ModelCoefficients::Ptr plane)
{
    float dp_distance=0;
    float sum =0;
    float module = sqrt(pow(plane->values[0],2)+ pow(plane->values[1], 2) + pow(plane->values[2], 2));
    for(int i = 0; i < 4; i++){
                sum += dot[i] * plane->values[i];
    }

    dp_distance = sum / module ;
    cout << dp_distance <<endl;
    return dp_distance;
}

//求直线到面交点    短线生成的面 垂直于长线 的交点
int Line_Plane_point(std::vector<pcl::ModelCoefficients::Ptr> seg_coefficients_line, std::vector<pcl::PointXYZ> point_cross )
{
     int size = seg_coefficients_line.size();
    std::vector<std::pair<int, int >>  groups;
    pcl::PointXYZ cpoint;
    float angle = 0;
    
    for(int i = 0; i < size; i++)
    {   
        for(int j = i+1; j < size; j++ )
        {
            angle = Angle_plane(seg_coefficients_line[i] ,seg_coefficients_line[j]) ;
            if (angle>20)
            {
                // 获取相交线 组
                cout << i <<"," << j <<endl;
                groups.emplace_back(i, j); // 

            }
        }    
    }  
    for(int k=0; k<groups.size(); k++)
    {
        int  n1 = groups[k].first;
        int  n2 = groups[k].second;
        float x1 = seg_coefficients_line[n1]->values[0];
        float y1 = seg_coefficients_line[n1]->values[1];
        float z1 = seg_coefficients_line[n1]->values[2];
        float a1 = seg_coefficients_line[n1]->values[3];
        float b1 = seg_coefficients_line[n1]->values[4];
        float c1 = seg_coefficients_line[n1]->values[5];
        float x2 = seg_coefficients_line[n2]->values[0];
        float y2 = seg_coefficients_line[n2]->values[1];
        float z2 = seg_coefficients_line[n2]->values[2];
        float a2 = seg_coefficients_line[n2]->values[3];
        float b2 = seg_coefficients_line[n2]->values[4];
        float c2 = seg_coefficients_line[n2]->values[5];
        float c_point = (a2*(y1-y2) - b2*(x1 - x2))/((b2*a1) - (a2 * b1));
        cpoint.x = x1 + a1*c_point;
        cpoint.y = y1 + b1*c_point;
        cpoint.z = z1 + c1*c_point;
        point_cross.push_back(cpoint); 
        std::cout << cpoint <<std ::endl;
    }
    for (int i =0; i<point_cross.size(); i++)
    {
        for(int j=i+1 ; j<point_cross.size(); j++)
        {
        
        float point_distance =  sqrt(pow(point_cross[i].x-point_cross[j].x, 2)+pow(point_cross[i].y-point_cross[j].y, 2)+pow(point_cross[i].z-point_cross[j].z, 2));
        
        printf("线距离%f\n", point_distance);
        }
    }
    //std::vector<float>
    return 0;
}

#endif
